#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTMotor)
#pragma config(Hubs,  S2, HTServo,  HTServo,  HTServo,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     motorFL,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorFR,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorBL,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorBR,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorFan1,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorFan2,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     motorSweeper,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorEnc,      tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C1_1,    servoFL,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C1_2,    servoFR,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C1_3,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_1,    servoBL,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C2_2,    servoBR,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C2_3,    servoLoader,          tServoStandard)
#pragma config(Servo,  srvo_S2_C2_4,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo7,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_1,    servoSweeperArm,      tServoStandard)
#pragma config(Servo,  srvo_S2_C3_2,    servoSweeper1,        tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C3_3,    servoSweeper2,        tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C3_4,    servo8,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_5,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S2_C3_6,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S2_C4_1,    servoTubeWinch,       tServoStandard)
#pragma config(Servo,  srvo_S2_C4_2,    servoTubeSlide,       tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C4_3,    servoGrabber,         tServoNone)
#pragma config(Servo,  srvo_S2_C4_4,    servo14,              tServoNone)
#pragma config(Servo,  srvo_S2_C4_5,    servo15,              tServoNone)
#pragma config(Servo,  srvo_S2_C4_6,    servo16,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "joystickdriver.c"
#include "includes/manipulators.c"


task main()
{
	int joyY1;
	int joyY2;

	servo[servoSweeper1] = 128;
	servo[servoSweeper2] = 128;
	servo[servoSweeperArm] = 0;
	servo[servoGrabber] = 50;
	servo[servoTubeWinch] = 184;

	bool sweepers = false;
	bool ready = true;

	bool fanRunning   = false;
	bool fanReady     = true;
	float fanPower = 0.0;

	bool loader   = false;
	bool loaderReady     = true;
	int loaderPos = 17;

	waitForStart();
	while(true)
	{


		getJoystickSettings(joystick);

		joyY1 = joystick.joy1_y1;
		joyY2 = joystick.joy1_y2;

		if (abs(joyY1) > 20)
		{
			motor[motorFL] = -1 * joyY1 + (20 * sgn(joyY1));
			motor[motorBL] = -1 * joyY1 + (20 * sgn(joyY1));
		}
		else
		{
			motor[motorFL] = 0;
			motor[motorBL] = 0;
		}

		if (abs(joyY2) > 20)
		{
			motor[motorFR] = joyY2 - (20 * sgn(joyY2));
			motor[motorBR] = joyY2 - (20 * sgn(joyY2));
		}
		else
		{
			motor[motorFR] = 0;
			motor[motorBR] = 0;
		}


		if ((joy1Btn(3) == true) && sweepers)
		{
			motor[motorSweeper] = -100;
		}
		else
		{
			motor[motorSweeper] = 0;
		}

		if (joy1Btn(4) == true)
		{
			servo[servoGrabber] = 0;
		}
		else if (joy1Btn(1) == true)
		{
			servo[servoGrabber] = 50;
		}

		if ((joy1Btn(2) == true) && (!sweepers && ready))
		{
			ready = false;
			sweepers = true;
			servo[servoSweeperArm] = 127;
			servo[servoSweeper1] = 255;
			servo[servoSweeper2] = 255;
		}
		else if ((joy1Btn(2) == true) && (sweepers && ready))
		{
			ready = false;
			sweepers = false;
			servo[servoSweeperArm] = 0;
			servo[servoSweeper1] = 128;
			servo[servoSweeper2] = 128;
		}
		else if (joy1Btn(2) == false)
		{
			ready = true;
		}

		/*
		if ((joy2Btn(3) == true) && fanReady)
		{
			fanReady = false;
			if (fanRunning)
			{
				fanRunning = true;
				StartTask(RunFan);
			}
			else
			{
				fanRunning = false;
				StopFan();
			}
		}
		else if (joy2Btn(3) == false)
		{
			fanReady = true;
		}
		*/
		if (joy2Btn(3) == true)
		{
			fanPower = 0.0;
		}

		if (joystick.joy2_y1 > 20)
		{
			fanPower += 0.02;
		}
		motor[motorFan1] = fanPower;
		motor[motorFan2] = -1 * fanPower;


		if (( (joy2Btn(1) == true) || (joystick.joy1_TopHat == 2) )&& loaderReady)
		{
			loaderReady = false;
			loaderPos = (loader)? 249 : 17;
		}
		else if (joy2Btn(1) == false)
		{
			loaderReady = true;
		}

		if(joystick.joy2_x2 > 20)
		{
			loaderPos = 255 - 0.2 * joystick.joy2_x2;
		}
		else if(joystick.joy2_x2 < 20)
		{
			loaderPos = 0 - 0.2 * joystick.joy2_x2;
		}

		servo[servoLoader] = loaderPos;

		if ((joy2Btn(2) == true) || (joystick.joy1_TopHat == 4))
		{
			servo[servoTubeWinch] = 133;
		}
		else if ((joy2Btn(4) == true) || (joystick.joy1_TopHat == 4))
		{
			servo[servoTubeWinch] = 54;
		}


	}

}
